//
// Created by liuhuan on 2022/4/29.
//

#include "control/GndControl.h"

void GndControl::Start(const std::string &ip, const int &port) {
    udpClient = new UdpClient;

    Recv_CallBack callBack = std::bind(&GndControl::RecvData, this, std::placeholders::_1,
                                       std::placeholders::_2);
    udpClient->Start(ip, port, callBack);
    m_isStart = true;
    m_thread = new std::thread(startSendInfo, this);
}

void GndControl::Stop() {
    m_isStart = false;
    delete udpClient;
}

void GndControl::RecvData(char *data, size_t nLen) {
//    PrintBuffer(data, nLen);

    if (nLen < sizeof(COMMON_HEADER_S) + 2)//收到数据小于包头+校验码，不符合要求直接返回
    {
        return;
    }
    if ((unsigned char) (data[0]) != PACKHEADER_0 ||
        (unsigned char) (data[1]) != PACKHEADER_1)//检查包头
        return;

    COMMON_HEADER_S headerS;
    memcpy(&headerS, data, sizeof(COMMON_HEADER_S));

    switch (headerS.cmd) {
        case SkyToGndCmdEnum::returnDevInfo:
            _returnDevInfo(data, nLen);
            break;
        case SkyToGndCmdEnum::returnBaseInfo:
            _returnBaseInfo(data, nLen);
            break;
        case SkyToGndCmdEnum::returnDetectInfo:
            _returnDetectInfo(data, nLen);
            break;
        case SkyToGndCmdEnum::returnTrackInfo:
            _returnTrackInfo(data, nLen);
            break;
        case SkyToGndCmdEnum::returnPtzPidInfo:
            _returnPtzPidInfo(data, nLen);
            break;
        case SkyToGndCmdEnum::returnAlgParamsInfo:
            _returnAlgParamsInfo(data, nLen);
            break;
    }
}

unsigned short GndControl::getCrc(char *data, int len) {
    return crcCount(data + 2, len - 4);
}

void *GndControl::startSendInfo(void *pArgs) {
    auto *ptr = (GndControl *) pArgs;
    ptr->sendInfo();
    return nullptr;
}

void GndControl::sendInfo() {
    while (m_isStart) {
        unsigned char array[1];
        array[0] = 0x00;
        udpClient->WriteUdp((char *) array, 1);
        std::this_thread::sleep_for(std::chrono::seconds(60));
    }
}

void GndControl::_returnDetectInfo(char *data, size_t nLen) {
//    std::cout << ("_recvDetectBox") << std::endl;
    if (nLen < sizeof(SKYNODE_GND_DETECTINFOS_S))
        return;

    SKYNODE_GND_DETECTINFOS_S skynodeGndDetectinfosS;
    memcpy(&skynodeGndDetectinfosS, data, sizeof(SKYNODE_GND_DETECTINFOS_S));
    std::vector <BoundingBox> boxs(skynodeGndDetectinfosS.boxLen);

    memcpy(&boxs[0], data + sizeof(SKYNODE_GND_DETECTINFOS_S),
           sizeof(BoundingBox) * skynodeGndDetectinfosS.boxLen);
    CallBack::getInstance()->detectInfoCB(skynodeGndDetectinfosS, boxs);
}

void GndControl::_returnBaseInfo(char *data, size_t nLen) {
    if (nLen < sizeof(SKYNODE_GND_BASEINFO_S))
        return;
    SKYNODE_GND_BASEINFO_S skynodeGndBaseinfoS;
    memcpy(&skynodeGndBaseinfoS, data, sizeof(SKYNODE_GND_BASEINFO_S));
    CallBack::getInstance()->baseInfoCB(skynodeGndBaseinfoS);
//    if (m_baseInfoCb != nullptr)
//        m_baseInfoCb(skynodeGndBaseinfoS);
}

void GndControl::_returnDevInfo(char *data, size_t nLen) {
    GND_LOGD("_returnDevInfo")
    if (nLen < sizeof(SKYNODE_GND_DEVINFO_S))
        return;

    SKYNODE_GND_DEVINFO_S skynodeGndDevinfoS;
    memcpy(&skynodeGndDevinfoS, data, sizeof(SKYNODE_GND_DEVINFO_S));
    if (skynodeGndDevinfoS.devNums <= 0)//如果没有设备，就直接返回
        return;

    std::vector <DeviceInfo> devices(skynodeGndDevinfoS.devNums);
    memcpy(&devices[0], data + sizeof(SKYNODE_GND_DEVINFO_S), sizeof(DeviceInfo) * devices.size());
    for (int i = 0; i < devices.size(); ++i) {
        printf("Device (%d) NO %d CamType %d Port %d\n", i, devices[i].no, devices[i].camType,
               devices[i].port);
    }
    CallBack::getInstance()->devInfoCB(skynodeGndDevinfoS, devices);
//    if (m_devInfoCb != nullptr)
//        m_devInfoCb(skynodeGndDevinfoS, devices);
}

void GndControl::_returnTrackInfo(char *data, size_t nLen) {
    if (nLen < sizeof(SKYNODE_GND_TRACKINFO_S))
        return;
    SKYNODE_GND_TRACKINFO_S skynodeGndTrackinfoS;
    memcpy(&skynodeGndTrackinfoS, data, sizeof(SKYNODE_GND_TRACKINFO_S));
    if (skynodeGndTrackinfoS.destIsLost == TRUE)//如果目标都已经丢失，就直接返回了
        return;
    CallBack::getInstance()->trackInfoCB(skynodeGndTrackinfoS);
//    if (m_trackInfoCb != nullptr)
//        m_trackInfoCb(skynodeGndTrackinfoS);
}

void GndControl::_returnPtzPidInfo(char *data, size_t nLen) {
    if (nLen < sizeof(SKYNODE_GND_PTZ_PID_S))
        return;
    SKYNODE_GND_PTZ_PID_S skyNodeGndPtzPID;
    memcpy(&skyNodeGndPtzPID, data, sizeof(SKYNODE_GND_PTZ_PID_S));

    CallBack::getInstance()->ptzPIDInfoCB(skyNodeGndPtzPID);
//    if (m_ptzPIDInfoCb != nullptr) {
//        m_ptzPIDInfoCb(skyNodeGndPtzPID);
//    }
}

void GndControl::_returnAlgParamsInfo(char *data, size_t nLen) {
    if (nLen < sizeof(SKYNODE_GND_ALG_PARAM_S))
        return;
    SKYNODE_GND_ALG_PARAM_S skyNodeGndPtzPID;
    memcpy(&skyNodeGndPtzPID, data, sizeof(SKYNODE_GND_ALG_PARAM_S));
    GND_LOGD("_returnAlgParamsInfo")
    CallBack::getInstance()->algParamInfoCB(skyNodeGndPtzPID);
//    if (m_algParamInfo != nullptr) {
//        m_algParamInfo(skyNodeGndPtzPID);
//    }
}


void GndControl::QuerySetDev() {
    GND_LOGD("Send QuerySetDev")

    GND_SKYNODE_QUERYDEV_S gndSkynodeQuerydevS;
    gndSkynodeQuerydevS.header.len = sizeof(gndSkynodeQuerydevS);
    gndSkynodeQuerydevS.header.identificationCode[0] = 0x13;
    gndSkynodeQuerydevS.header.identificationCode[1] = 0x13;
    gndSkynodeQuerydevS.header.cmd = GndToSkyCmdEnum::querySetDev;

    gndSkynodeQuerydevS.infoReturnFreq = 30;
    gndSkynodeQuerydevS.crc = getCrc((char *) &gndSkynodeQuerydevS, sizeof(gndSkynodeQuerydevS));

//    PrintBuffer("QuerySetDev", (char *)&gndSkynodeQuerydevS, sizeof(gndSkynodeQuerydevS));
    udpClient->WriteUdp((char *) &gndSkynodeQuerydevS, sizeof(gndSkynodeQuerydevS));
}

void GndControl::QueryAlgParams(unsigned char devNo) {
    GND_SKYNODE_QUERY_ALG_PARAMS_STRU gndSkynodeQuerydevS;
    gndSkynodeQuerydevS.header.len = sizeof(gndSkynodeQuerydevS);
    gndSkynodeQuerydevS.header.identificationCode[0] = 0x13;
    gndSkynodeQuerydevS.header.identificationCode[1] = 0x13;
    gndSkynodeQuerydevS.header.cmd = GndToSkyCmdEnum::queryAlgParams;

    gndSkynodeQuerydevS.deviceNo = devNo;
    gndSkynodeQuerydevS.crc = getCrc((char *) &gndSkynodeQuerydevS, sizeof(gndSkynodeQuerydevS));

//    PrintBuffer("QueryAlgParams", (char *)&gndSkynodeQuerydevS, sizeof(gndSkynodeQuerydevS));
    udpClient->WriteUdp((char *) &gndSkynodeQuerydevS, sizeof(gndSkynodeQuerydevS));
}

void GndControl::QueryTrackPtzPid() {
    GND_SKYNODE_QUERYPID_S gndSkyNodeQuqeyPID;
    gndSkyNodeQuqeyPID.header.len = sizeof(GND_SKYNODE_QUERYPID_S) - 2;
    gndSkyNodeQuqeyPID.header.identificationCode[0] = 0x13;
    gndSkyNodeQuqeyPID.header.identificationCode[1] = 0x13;
    gndSkyNodeQuqeyPID.header.cmd = GndToSkyCmdEnum::queryPtzPid;
    gndSkyNodeQuqeyPID.crc = getCrc((char *) &gndSkyNodeQuqeyPID, sizeof(GND_SKYNODE_QUERYPID_S));
//    PrintBuffer("QueryTrackPtzPid", (char *)&gndSkyNodeQuqeyPID, sizeof(gndSkyNodeQuqeyPID));
    udpClient->WriteUdp((char *) &gndSkyNodeQuqeyPID, sizeof(gndSkyNodeQuqeyPID));
}

void GndControl::PtzMove(GndToSkyPtzTypeEnum ptzType, unsigned char direction, unsigned short angle,
                         unsigned char speed) {
    GND_LOGD("Send PtzMove")
    GND_SKYNODE_PTZ_S gndSkynodePtzS;
    gndSkynodePtzS.header.len = sizeof(gndSkynodePtzS);
    gndSkynodePtzS.header.identificationCode[0] = 0x13;
    gndSkynodePtzS.header.identificationCode[1] = 0x13;
    gndSkynodePtzS.header.cmd = GndToSkyCmdEnum::ptzMove;

    gndSkynodePtzS.ptzType = ptzType;
    gndSkynodePtzS.direction = direction;
    gndSkynodePtzS.angle = angle;
    gndSkynodePtzS.speed = speed;

//    PrintBuffer("PtzMove", (char *)&gndSkynodePtzS, sizeof(gndSkynodePtzS));
    gndSkynodePtzS.crc = getCrc((char *) &gndSkynodePtzS, sizeof(gndSkynodePtzS));

    udpClient->WriteUdp((char *) &gndSkynodePtzS, sizeof(gndSkynodePtzS));
}

void GndControl::ZoomAction(unsigned char devNo, unsigned char direction) {
    GND_LOGD("Send ZoomAction")
    GND_SKYNODE_ZOOM_S gndSkyNodeZooms;
    gndSkyNodeZooms.header.len = sizeof(GND_SKYNODE_ZOOM_S);
    gndSkyNodeZooms.header.identificationCode[0] = 0x13;
    gndSkyNodeZooms.header.identificationCode[1] = 0x13;
    gndSkyNodeZooms.header.cmd = GndToSkyCmdEnum::camZoom;

    gndSkyNodeZooms.devNo = devNo;
    gndSkyNodeZooms.direction = direction;

    gndSkyNodeZooms.crc = getCrc((char *) &gndSkyNodeZooms, sizeof(gndSkyNodeZooms));

    udpClient->WriteUdp((char *) &gndSkyNodeZooms, sizeof(gndSkyNodeZooms));
}

void GndControl::TrackAction(unsigned char devNo, BoundingBox box) {
    std::cout << "Send TrackAction" << std::endl;
    GND_SKYNODE_SETTRACKMODE_S gndSkynodeSettrackmodeS;

    gndSkynodeSettrackmodeS.header.len = sizeof(GND_SKYNODE_ZOOM_S);
    gndSkynodeSettrackmodeS.header.identificationCode[0] = 0x13;
    gndSkynodeSettrackmodeS.header.identificationCode[1] = 0x13;
    gndSkynodeSettrackmodeS.header.cmd = GndToSkyCmdEnum::setTrackMode;

    gndSkynodeSettrackmodeS.devNo = devNo;
    gndSkynodeSettrackmodeS.isMovePtz = 0x01;
    gndSkynodeSettrackmodeS.xMin = box.xmin;
    gndSkynodeSettrackmodeS.yMin = box.ymin;
    gndSkynodeSettrackmodeS.xMax = box.xmax;
    gndSkynodeSettrackmodeS.yMax = box.ymax;

    gndSkynodeSettrackmodeS.crc = getCrc((char *) &gndSkynodeSettrackmodeS,
                                         sizeof(gndSkynodeSettrackmodeS));

    udpClient->WriteUdp((char *) &gndSkynodeSettrackmodeS, sizeof(gndSkynodeSettrackmodeS));
}

void GndControl::SetPtzPID(PIDInfo info, GndToSkyPtzPIDEnum type) {
    GND_SKYNODE_SETPTZ_PID_S gndSkyNodeSetPID;
    gndSkyNodeSetPID.header.len = sizeof(GND_SKYNODE_SETPTZ_PID_S) - 2;
    gndSkyNodeSetPID.header.identificationCode[0] = 0x13;
    gndSkyNodeSetPID.header.identificationCode[1] = 0x13;
    gndSkyNodeSetPID.header.cmd = GndToSkyCmdEnum::updatePtzPid;
    gndSkyNodeSetPID.type = type;
    gndSkyNodeSetPID.p = info.p;
    gndSkyNodeSetPID.i = info.i;
    gndSkyNodeSetPID.d = info.d;
    gndSkyNodeSetPID.crc = getCrc((char *) &gndSkyNodeSetPID, sizeof(GND_SKYNODE_SETPTZ_PID_S));
//    PrintBuffer("SetPtzPID", (char *) &gndSkyNodeSetPID, sizeof(gndSkyNodeSetPID));
    udpClient->WriteUdp((char *) &gndSkyNodeSetPID, sizeof(gndSkyNodeSetPID));
}

void GndControl::SetDetectParams(DetectParams params, unsigned char devNo) {
    GND_SKYNODE_UPDATE_PARAMS_STRU gnd_skynode_detect_params_s;
    gnd_skynode_detect_params_s.header.cmd = GndToSkyCmdEnum::updateAlgParams;
    gnd_skynode_detect_params_s.header.len = sizeof(GND_SKYNODE_UPDATE_PARAMS_STRU) - 2;
    gnd_skynode_detect_params_s.header.identificationCode[0] = 0x13;
    gnd_skynode_detect_params_s.header.identificationCode[1] = 0x13;
    gnd_skynode_detect_params_s.devNo = devNo;
    gnd_skynode_detect_params_s.type = GndToSkyAlgParamsEnum::params_detect;
    gnd_skynode_detect_params_s.detectParams = params;
    gnd_skynode_detect_params_s.crc = getCrc((char *) &gnd_skynode_detect_params_s,
                                             sizeof(gnd_skynode_detect_params_s));
    udpClient->WriteUdp((char *) &gnd_skynode_detect_params_s, sizeof(gnd_skynode_detect_params_s));
}

void GndControl::SetTrackParams(TrackParams params, unsigned char devNo) {
    GND_SKYNODE_UPDATE_PARAMS_STRU gnd_skynode_detect_params_s;
    gnd_skynode_detect_params_s.header.cmd = GndToSkyCmdEnum::updateAlgParams;
    gnd_skynode_detect_params_s.header.len = sizeof(GND_SKYNODE_UPDATE_PARAMS_STRU) - 2;
    gnd_skynode_detect_params_s.header.identificationCode[0] = 0x13;
    gnd_skynode_detect_params_s.header.identificationCode[1] = 0x13;
    gnd_skynode_detect_params_s.devNo = devNo;
    gnd_skynode_detect_params_s.type = GndToSkyAlgParamsEnum::params_track;
    gnd_skynode_detect_params_s.trackParams = params;
    gnd_skynode_detect_params_s.crc = getCrc((char *) &gnd_skynode_detect_params_s,
                                             sizeof(gnd_skynode_detect_params_s));
    udpClient->WriteUdp((char *) &gnd_skynode_detect_params_s, sizeof(gnd_skynode_detect_params_s));
}

void GndControl::SetOneShot(std::string imgPath, unsigned char devNo) {
//    cv::Mat img = cv::imread(imgPath.c_str());
//    cv::resize(img, img, cv::Size(608, 352));
//    int buffersize = 608 * 352 * 3;
//    unsigned char *imgBuffer = img.data;
//
//    //cv::Mat img2(352, 608, CV_8UC3, imgData);
//    //cv::imwrite("cv2.png", img2);
//
//    GND_SKYNODE_SETONESHOTMODE_S oneShotS;
//    oneShotS.header.cmd = GndToSkyCmdEnum::setOneShotMode;
//    oneShotS.header.identificationCode[0] = 0x13;
//    oneShotS.header.identificationCode[1] = 0x13;
//    oneShotS.devNo = devNo;
//    oneShotS.isMovePtz = TRUE;
//    oneShotS.crc = 0;
//
//    int ret;
//
//    IMAGE_S imageS;
//    imageS.packTotalNum = buffersize % IMAGEPACKSIZE == 0 ? (buffersize / IMAGEPACKSIZE) : (buffersize / IMAGEPACKSIZE + 1);//如果有余数就+1
////    imageS.image_id = 10 ;
//    imageS.imageTotalSize = buffersize;
//    imageS.imgCrc = crcCount((char*)imgBuffer, buffersize);
//
//    std::cout << "imgLen: " << imageS.imageTotalSize << std::endl;
//    std::cout << "imageS.packTotalNum: " << imageS.packTotalNum;
//
//    //分包连续发送
//    int dataOffset;
//    for (int i = 0; i < imageS.packTotalNum; i++)
//    {
//        dataOffset = IMAGEPACKSIZE * i;
//        memcpy(imageS.imageData, imgBuffer + dataOffset, IMAGEPACKSIZE);
//        imageS.packCurrIndex = i;
//        oneShotS.imgDataPack = imageS;
//        oneShotS.crc = getCrc((char*)&oneShotS, sizeof(GND_SKYNODE_SETONESHOTMODE_S));
//        int ret = udpClient->WriteUdp((char*)&oneShotS, sizeof(GND_SKYNODE_SETONESHOTMODE_S));
//
//        ////需要增加发送是否成功判断
//        ////
//        printf("sending: %d / %d\n", imageS.packCurrIndex, imageS.packTotalNum);
//
//        Sleep(1);//ms
//    }
//
//    //qDebug() << "send img success, spend time: " << sTime.msecsTo(QTime::currentTime()) << "ms";
//
//
//    std::cout << "img send success." << std::endl;
//    std::cout << "sizeof(GND_SKYNODE_SETONESHOTMODE_S):" << sizeof(GND_SKYNODE_SETONESHOTMODE_S) << std::endl;
//    std::cout << "sizeof(image_stru):" << sizeof(image_stru) << std::endl;
}
